#include <stdbool.h>
#include "hw_types.h"
#include "hw_memmap.h"
#include "hw_ints.h"
#include "src/debug.h"
#include "src/interrupt.h"
#include "src/sysctl.h"
#include "src/systick.h"
#include "src/gpio.h"
#include "src/uart.h"
#include "src/timer.h"
#include "utils/diag.h"

#ifdef DEBUG
void
__error__(char *pcFilename, unsigned long ulLine)
{
}
#endif

unsigned long _ulDistance=0xffff;
unsigned long _ulRuntimeMs=0;
int _iStep;
const int _ciSystickMs=100;
unsigned long _ulCyclesPerCm;
bool _bStopped=true;

void sleep(int ms){
  SysCtlDelay(SysCtlClockGet()/1000*ms);
}

void drive(int motor, bool forward, int speed){
    UARTCharPut(UART0_BASE, 0x80);
    UARTCharPut(UART0_BASE, 0x0);
    UARTCharPut(UART0_BASE, (forward?1:0)+motor*2);
    UARTCharPut(UART0_BASE, speed); 
}

void
SysTickIntHandler(void)
{
  _ulRuntimeMs+=_ciSystickMs;
  if (_bStopped){
    if (GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_0)==0){
      _bStopped=false;
      // reset motor driver
      GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_4, 0);
      sleep(1);
      GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_4, GPIO_PIN_4);
    }
  } else {
  if (GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_1)==0){
      _bStopped=true;
      drive(0, 0, 0x0);
      drive(1, 0, 0x0);
      return;
    }
  GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_2, ~GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_2));
  if (_ulDistance<20){
    drive(0, (_ulRuntimeMs/5000)%2, 0x30);
    drive(1, !((_ulRuntimeMs/5000)%2), 0x30);
  } else if (_ulDistance<25){
    drive(0, true, 0x10);
    drive(1, true, 0x10);
  } else if (_ulDistance<30){
    drive(0, true, 0x20);
    drive(1, true, 0x20);
  } else if (_ulDistance<70){
    drive(0, true, 0x30);
    drive(1, true, 0x30);
  } else {
    drive(0, true, 0x40);
    drive(1, true, 0x40);
  }
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
    GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_0);
    GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_0, 1);
    sleep(1);
    GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_0, 0);
    GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_0);
    _iStep=0;
    TimerEnable(TIMER0_BASE, TIMER_A);
  }
}

// The UART interrupt handler.
void
UARTIntHandler(void)
{}


void
Timer0IntHandler(void)
{
  unsigned long ulDistance;
  TimerIntClear(TIMER0_BASE, TIMER_CAPA_EVENT|TIMER_TIMA_TIMEOUT);
  
  ulDistance=(0xffff-TimerValueGet(TIMER0_BASE, TIMER_A))/_ulCyclesPerCm;
  TimerDisable(TIMER0_BASE, TIMER_A);
  TimerLoadSet(TIMER0_BASE, TIMER_A, 0xffff);
  if (_iStep==0){
    _iStep=1;
    TimerEnable(TIMER0_BASE, TIMER_A);
  } else {
    _ulDistance=ulDistance;
  }
}


int
main(void)
{
    //
    // Set the clocking to run directly from the crystal.
    //
    SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
                   SYSCTL_XTAL_8MHZ);
      
    //
    // Configure GPIO Pin used for the LED.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
    GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2);
    GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE, GPIO_PIN_4);
    GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_4, 0);
    sleep(1);
    GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_4, GPIO_PIN_4);
      
    SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
    
    //
    // Enable the pull-ups on the JTAG signals.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
    GPIOPadConfigSet(GPIO_PORTC_BASE,
                     GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3| GPIO_PIN_4,
                     GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);

    //
    // Configure GPIO Pins used for the Buttons.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, GPIO_PIN_0 | GPIO_PIN_1);
    GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_0 | GPIO_PIN_1,
                     GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);  
    //
    // Enable processor interrupts.
    //
    IntMasterEnable();
    
    TimerConfigure(TIMER0_BASE, TIMER_CFG_16_BIT_PAIR|TIMER_CFG_A_CAP_TIME);
    TimerControlEvent(TIMER0_BASE, TIMER_A, TIMER_EVENT_BOTH_EDGES);
    TimerPrescaleSet(TIMER0_BASE, TIMER_A, 10);
    // according to srf05 docs - divide uS by 58 to get cm
    _ulCyclesPerCm=58*(SysCtlClockGet()/1000000);
    //TimerLoadSet(TIMER0_BASE, TIMER_A, SysCtlClockGet());
    //TimerIntRegister(TIMER0_BASE, TIMER_A, Timer0IntHandler);
    IntEnable(INT_TIMER0A);
    TimerIntEnable(TIMER0_BASE, TIMER_CAPA_EVENT|TIMER_TIMA_TIMEOUT);
           
    //
    // Set GPIO A0 and A1 as UART pins.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
    GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

    //
    // Configure the UART for 9600, 8-N-1 operation.
    //
    UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 1200,
                        (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
                         UART_CONFIG_PAR_NONE));

    //
    // Enable the UART interrupt.
    //
    IntEnable(INT_UART0);
    //
    // Configure SysTick for a 1000ms interrupt.
    //
    SysTickPeriodSet(SysCtlClockGet()/1000*_ciSystickMs);
    SysTickEnable();
    SysTickIntEnable();
    
    // begin execution
    drive(0, true, 0x0);
    drive(1, true, 0x0);
    while(1){sleep(100);}
    if (0){
    sleep(1000);
    
    UARTCharPut(UART0_BASE, 0x80);
    UARTCharPut(UART0_BASE, 0x2);
    UARTCharPut(UART0_BASE, 0x2);
    while (1){}
    }
 
    sleep(1000);
    
    drive(0, true, 0x30);
    drive(1, true, 0x30);
    
    sleep(2000);
    
    drive(0, true, 0x20);
    drive(1, true, 0x40);
    
    sleep(2000);
    
    drive(0, true, 0x50);
    drive(1, true, 0x50);
    
    sleep(2000);
    
    drive(0, false, 0x30);
    drive(1, true, 0x30);
   
    sleep(1000);
    
    drive(0, false, 0x40);
    drive(1, false, 0x20);
    
    sleep(2000);
    
    drive(0, true, 0);
    drive(1, true, 0);
    //
    // Loop forever.
    //
    while(1)
    {
    }
}